/*
@brief:发送之间要有间隔 因为是要在空气传播的 可能你发出了 但是没有传播完。
@brief:现在暂时都设置继电器的从机地址都是 1而且每次都是写一个线圈寄存器。
@brief:默认一个DTU就只连接一个继电器（电梯控制）。
@brief:id其实是DTU的ID，主要是操作DTU去控制继电器。
@brief:读取是否开门(开了门才能离开进入) 读取是否到达楼层(到了才能开门)
@brief:Y是输出 X是读取
@brief: Y1:开门 Y2:关门 Y3:楼层1 Y4:楼层2 Y5:楼层3
@brief: X1:读是否开门 X2-X4:读是否到楼层(1-3楼)
*/
#include "elerator_uart.h"
#include "sbus.h"
#include "bsp_usart.h"
using namespace man;
Elevator elevator_1(1);//电梯对象 1
Elevator elevator_2(2);//电梯2 对象2
extern DMA_HandleTypeDef hdma_usart3_rx;
extern uint8_t uart3_txbuf[32];
static uint8_t msgReceiveArr[15]={0};
/*
@brief: 类构造函数
@param: id指定是哪一个继电器（可理解为电梯）
*/

Elevator::Elevator(uint8_t id)
:	floor(-1),
	isTargetFloorSet_(false),
	elevatorStatus(WAIT_OPEN),
	isOpenedDoor(false),
	isReachedTargetFloor(false),
	isNeedElevator(false),
	isEnterElevator(false),
	isLeaveElevator(false),
	currentQueryAddress(-1),
	elevatorID(id)
	,isCalled(false)
	,currentFloor(-1)
	,countTime(0)
	,isFirstEntry(true)
{
	
}	

void  Elevator::init(){
	HAL_UARTEx_ReceiveToIdle_DMA(&huart3,msgReceiveArr,sizeof(msgReceiveArr));//开启DMA 空闲中断
	__HAL_DMA_DISABLE_IT(&hdma_usart3_rx,DMA_IT_HT);//过半传输失能,过半也会调用这个event回调函
}
void Elevator::close(uint8_t id){
	// to do
	
	/*
	使用static 是因为DMA要到内存中转运数据 
	如果你的数据出了作用域就销毁 那么这个地址就非法了。
	存放在静态存储区
	*/
	static uint8_t arr[8];
	arr[0]=id;
	arr[1]=0x05;//指令码 写单个线圈
	arr[2]=COIL_REG_COLSE>>8;//寄存器地址 高字节
	arr[3]=COIL_REG_COLSE;//寄存器地址 低字节
	arr[4]=0;//值 高字节
	arr[5]=0;//值 低字节
	GetModbusCRC16(arr,6,&arr[6],&arr[7]);
	HAL_StatusTypeDef uartStatus=HAL_OK;
	do{
		uartStatus=HAL_UART_Transmit_DMA(&huart3,arr,8);
	}while(uartStatus==HAL_BUSY);
	HAL_Delay(200);
}

/*
@brief: 用于电梯开门
@param: id指定是哪一个电梯
*/
void Elevator::open(uint8_t id){
	//to do
	static uint8_t arr[8];
	arr[0]=id;
	arr[1]=0x05;//指令码 写单个线圈
	arr[2]=COIL_REG_OPEN>>8;//寄存器地址 高字节
	arr[3]=COIL_REG_OPEN;//寄存器地址 低字节
	arr[4]=0xFF;//值 高字节
	arr[5]=0;//值 低字节
	GetModbusCRC16((uint8_t *)arr,6,(uint8_t*)&arr[6],(uint8_t *)&arr[7]);
	HAL_StatusTypeDef uartStatus=HAL_OK;
	do{
		uartStatus=HAL_UART_Transmit_DMA(&huart3,arr,8);
	}while(uartStatus==HAL_BUSY);
	HAL_Delay(200);
}


/*
@brief: 用于电梯设置楼层
@param: id指定是哪一个电梯
@param:floor 指定哪一楼层
*/
void Elevator::setFloor(uint8_t id,uint8_t floor){
	//to do
	isTargetFloorSet_=true;
	uint32_t address=0;
	this->floor=floor;
	
	switch(floor){
		case 1:
			address=COIL_REG_FLOOR_1;
			break;
		case 2:
			address=COIL_REG_FLOOR_2;
			break;
		case 3:
			address=COIL_REG_FLOOR_3;
			break;
	}
	
	static uint8_t arr[8];
	arr[0]=id;
	arr[1]=0x05;//指令码 写单个线圈
	arr[2]=address>>8;//寄存器地址 高字节
	arr[3]=address;//寄存器地址 低字节
	arr[4]=0xFF;//值 高字节
	arr[5]=0;//值 低字节
	GetModbusCRC16(arr,6,&arr[6],&arr[7]);//crc 校验码
	HAL_StatusTypeDef uartStatus=HAL_OK;
	
	
	do{
		uartStatus=HAL_UART_Transmit_DMA(&huart3,arr,8);
	}while(uartStatus==HAL_BUSY);
	
	HAL_Delay(200);
	
}

bool Elevator::hasReachedTargetFloor(){
	/*
		读取离散输入
		要先设置楼层才能询问有没有到达目标楼层 不然逻辑会混乱
	*/
	if(!isTargetFloorSet_)//判断设置了楼层没有
		return false;
	if(!isReachedTargetFloor){
		//继续访问是否到达了楼层。 根据floor_询问指定寄存器
		switch(floor){
			case 1:
				currentQueryAddress=DIN_REG_FLOOR_1;
				break;
			case 2:
				currentQueryAddress=DIN_REG_FLOOR_2;
				break;
			case 3:
				currentQueryAddress=DIN_REG_FLOOR_3;
				break;
		}
		static uint8_t arr[8];
		arr[0]=elevatorID;//从机地址 电梯ID||继电器ID
		arr[1]=0x02;//指令码 离散输入寄存器
		arr[2]=currentQueryAddress>>8;//寄存器地址高字节
		arr[3]=currentQueryAddress;//寄存器地址低字节
		arr[4]=0x00;//读取数量高字节
		arr[5]=0x01;//读取数量低字节
		GetModbusCRC16(arr,6,&arr[6],&arr[7]);//crc 校验码
		HAL_StatusTypeDef uartStatus=HAL_OK;
		do{
			uartStatus=HAL_UART_Transmit_DMA(&huart3,arr,8);
		}while(uartStatus==HAL_BUSY);
		HAL_Delay(200);
		return false;
	}
	else{
		/*
		发送到达指定楼层指令
		*/ 
		
		isReachedTargetFloor=false;//状态更新
		return true;
	}
	
}

bool Elevator::hasOpenedDoor(){
	//读取离散输入寄存器 DIN_REG_OPEN
	if(!isOpenedDoor){
		static uint8_t arr[8];
		currentQueryAddress= DIN_REG_OPEN;//离散输入寄存器地址
		arr[0]=elevatorID;//从机地址 电梯ID||继电器ID
		arr[1]=0x02;//指令码 读离散寄存器
		arr[2]=currentQueryAddress>>8;//寄存器地址高字节
		arr[3]=currentQueryAddress;//寄存器地址低字节
		arr[4]=0;//读取数量高字节
		arr[5]=1;//读取数量低字节
		GetModbusCRC16(arr,6,&arr[6],&arr[7]);//crc 校验码
		HAL_StatusTypeDef uartStatus=HAL_OK;
		do{
			uartStatus=HAL_UART_Transmit_DMA(&huart3,arr,8);
			INFO("%c\n",'B');
		}while(uartStatus==HAL_BUSY);
		HAL_Delay(200);
		return false;
	}else{
		//to do 告诉上位机开门了
		isOpenedDoor=false;
	}
	
	return true;
}
bool Elevator::hasLeaveElevator(){
	/*
	上位机离开了会发送一个离开信号
	*/
	if(isLeaveElevator){
		isLeaveElevator=!isLeaveElevator;
		return true;
	}
	return false;
}
bool Elevator::hasEnternElevator(){
	/*
	上位机进入了会发送一个进入信号
	*/
	if(isEnterElevator){
		isEnterElevator=!isEnterElevator;
		return true;
	}
	return false;
}
void Elevator::callElevator(uint8_t id,int8_t dir){
	/*
	呼叫电梯
	根据dir写入不同的线圈寄存器
	*/
	static uint8_t arr[8];
	uint32_t address=0;
	if(dir==UP){
		address=COIL_REG_UP;
	}else{
		address=COIL_REG_DOWN;//线圈寄存器DOWN
	}
	arr[0]=id;//电梯ID
	arr[1]=0x05;//指令码 写单个线圈
	arr[2]=address>>8;//寄存器地址 高字节
	arr[3]=address;//寄存器地址 低字节
	arr[4]=0xFF;//值 高字节
	arr[5]=0;//值 低字节
	GetModbusCRC16(arr,6,&arr[6],&arr[7]);//crc 校验码
	HAL_StatusTypeDef uartStatus=HAL_OK;
	do{
		uartStatus=HAL_UART_Transmit_DMA(&huart3,arr,8);
	}while(uartStatus==HAL_BUSY);
	HAL_Delay(200);
}

/*
用于重置线圈寄存器的值。 默认为0
*/
void Elevator::resetElevatorReg(uint8_t id,Register coilReg){
		
	static uint8_t arr[8];
	arr[0]=id;
	arr[1]=0x05;//指令码 写单个线圈
	arr[2]=coilReg>>8;//寄存器地址 高字节
	arr[3]=coilReg;//寄存器地址 低字节
	arr[4]=0;//值 高字节
	arr[5]=0;//值 低字节
	GetModbusCRC16((uint8_t *)arr,6,(uint8_t*)&arr[6],(uint8_t *)&arr[7]);
	HAL_StatusTypeDef uartStatus=HAL_OK;
	do{
		uartStatus=HAL_UART_Transmit_DMA(&huart3,arr,8);
	}while(uartStatus==HAL_BUSY);
	HAL_Delay(200);
}
/*
@brief:接口函数 进行控制电梯
@brief:
1.先呼叫电梯（让电梯来当前楼层），检测寄存器状态判断是否开了门且当前当前楼层
，如果到达当前楼层那么我重置当前楼层的继电器状态且，我让他保持开门状态直到机器人进入到电梯。
2.机器人进入到电梯了，我重置开门状态 设置关门 和设置目标楼层。进到了才进行设置。
3.读取继电器状态判断是否到达了当前楼层，如果到达了那么进行开门，等待机器人离开电梯,开门了才可以离开
4.机器人离开电梯后我设置重置开门状态 且关门 状态机回到最开始。
*/
void Elevator::controlElevator(){
	if(isNeedElevator){
			switch(elevatorStatus){
				case man::WAIT_OPEN:
				{
					
					//呼叫电梯到达楼层 设置电梯来当前楼层
					if(!isCalled){
						setFloor(elevatorID,currentFloor);
					}
					//判断是否第一次进入这个CASE 补丁
					if(isFirstEntry){
						isFirstEntry=false;
						countTime=HAL_GetTick();
					}
					else{
						//如果等待时间太长 则视为错误 超过3分钟则认为错误
						if(abs(HAL_GetTick()-countTime)>180000){
							elevatorStatus=(ElevatorStatus)-1;//errow状态
							break;
						}
						
					}
					
					if(hasOpenedDoor()&&hasReachedTargetFloor()){//读取开门了没有并且在当前楼层
						//电梯开了门且在当前楼层才可以进去。
						uart3_txbuf[10]=0x01;//发送一个进去了的
						open(elevatorID);//保持开门状态。
						isCalled=false;//状态转化。
						isFirstEntry=true;//状态更新
						elevatorStatus=man::ENTERING_ELEVATOR;
					}
					break;
				}
				
				case man::ENTERING_ELEVATOR:
				{
					//判断是否第一次进入这个CASE 补丁
					if(isFirstEntry){
						isFirstEntry=false;
						countTime=HAL_GetTick();
					}
					else{
						//如果等待时间太长 则视为错误 超过3分钟则认为错误
						if(abs(HAL_GetTick()-countTime)>180000){
							elevatorStatus=(ElevatorStatus)-1;
							break;
						}
					}
					
					if(hasEnternElevator()){
						//如果已经进去了
						switch(currentFloor){
							case 1:
								resetElevatorReg(elevatorID,COIL_REG_FLOOR_1);
								break;
							case 2:
								resetElevatorReg(elevatorID,COIL_REG_FLOOR_2);
								break;
							case 3:
								resetElevatorReg(elevatorID,COIL_REG_FLOOR_3);
							break;
						}
						resetElevatorReg(elevatorID,COIL_REG_OPEN);//重置开门状态
						close(elevatorID);//关门
						resetElevatorReg(elevatorID,COIL_REG_COLSE);//重置关门状态
						isFirstEntry=true;
						elevatorStatus=man::SET_FLOOR;
					}
					break;
				}
				
				case man::SET_FLOOR:
				{
					//等待串口中断传输完数据才进行设置。
					setFloor(elevatorID,floor);//设置楼层要在串口中进行接收
					elevatorStatus=man::WAIT_REACHED;//等待到达
					isReachedTargetFloor=false;
					break;
				}
				case man::WAIT_REACHED:
				{
					//等待到达楼层
					//判断是否第一次进入这个CASE
					if(isFirstEntry){
						isFirstEntry=false;
						countTime=HAL_GetTick();
					}
					else{
						//如果等待时间太长 则视为错误 超过3分钟则认为错误
						if(abs(HAL_GetTick()-countTime)>180000){
							elevatorStatus=(ElevatorStatus)-1;
							break;
						}
					}
					//读取继电器状态
					if(hasReachedTargetFloor()){
						switch(floor){
							case 1:
								resetElevatorReg(elevatorID,COIL_REG_FLOOR_1);
								break;
							case 2:
								resetElevatorReg(elevatorID,COIL_REG_FLOOR_2);
								break;
							case 3:
								resetElevatorReg(elevatorID,COIL_REG_FLOOR_3);
								break;
						}
						elevatorStatus=man::REACHED_FLOOR;//到达楼层
						isFirstEntry=true;
					}
					break;
				}
				
				case man::REACHED_FLOOR:
				{
					//已经到达了 电梯会自动开门 我需要保持开门状态。
					//open(elevatorID);//开门
					
					//判断是否第一次进入这个CASE
					if(isFirstEntry){
						isFirstEntry=false;
						countTime=HAL_GetTick();
					}
					else{
						//如果等待时间太长 则视为错误 超过3分钟则认为错误
						if(abs(HAL_GetTick()-countTime)>180000){
							elevatorStatus=(ElevatorStatus)-1;
							break;
						}
					}
					
					if(hasOpenedDoor()){
						//电梯开了门才告诉上位机可以离开
						open(elevatorID);//保持开门
						uart3_txbuf[10]=0x02;
						elevatorStatus=man::LEAVE_ELEVATOR;//离开楼层
						isFirstEntry=true;
					}
					break;
				}
				
				case man::LEAVE_ELEVATOR:
				{
					//判断是否第一次进入这个CASE
					if(isFirstEntry){
						isFirstEntry=false;
						countTime=HAL_GetTick();
					}
					else{
						//如果等待时间太长 则视为错误 超过3分钟则认为错误
						if(abs(HAL_GetTick()-countTime)>180000){
							elevatorStatus=(ElevatorStatus)-1;
							break;
						}
					}
					
					if(hasLeaveElevator()){//已经离开。
						//重置开门状态
						resetElevatorReg(elevatorID,COIL_REG_OPEN);
						close(elevatorID);//触发一下关门
						resetElevatorReg(elevatorID,COIL_REG_COLSE);
						elevatorStatus=man::FINISHED;
						isFirstEntry=true;
					}
					break;
				}
				case man::FINISHED:
				{
					isNeedElevator=false;//完成了则不需要电梯了
					elevatorStatus=man::WAIT_OPEN;//回到等待开始
					break;
				}
				default:
					//错误处理 防止卡死在状态机中
					resetElevator();
			}
		}
}

void Elevator::test(){
	
//	isNeedElevator=false;
	//setFloor(elevatorID,1);
	hasOpenedDoor();
//	hasReachedTargetFloor();
	
}
//重置状态。
void Elevator::resetElevator(){
	
	elevatorStatus=WAIT_OPEN;
	isNeedElevator=false;
	isCalled=false;
	isEnterElevator=false;
	isOpenedDoor=false;
	isLeaveElevator=false;
	isReachedTargetFloor=false;
	isFirstEntry=true;
	for(uint8_t i=0;i<=3;i++){
		resetElevatorReg(elevatorID,(Register)(COIL_REG_OPEN+i));
	}
	
}

uint32_t Elevator::abs(int num){
	return num>0?num:-num;
}

/*
@brief 用于编写中断回调函数
*/
extern "C"{
	void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size){
		if(huart->Instance==USART3){
			if(elevator_1.isNeedElevator){
				/*
				进行数据处理。 先判断指令码以及电梯ID
				ID 电梯/继电器: 是自己要访问的ID那么才进行处理。
				*/
				if(msgReceiveArr[0]==0x01){
					
						if(msgReceiveArr[1]==0x02){
						/*
							读离散输入寄存器
						*/
						if(elevator_1.currentQueryAddress==man::DIN_REG_OPEN){
							if(msgReceiveArr[3]&0x01){
								elevator_1.isOpenedDoor=true;
							}
						}
						else if(elevator_1.currentQueryAddress==man::DIN_REG_FLOOR_1){
							if(msgReceiveArr[3]&0x01){
								elevator_1.isReachedTargetFloor=true;
							}
						}else if(elevator_1.currentQueryAddress==man::DIN_REG_FLOOR_2){
							if(msgReceiveArr[3]&0x01){
								elevator_1.isReachedTargetFloor=true;
							}
						}else if(elevator_1.currentQueryAddress==man::DIN_REG_FLOOR_3){
							if(msgReceiveArr[3]&0x01){
								elevator_1.isReachedTargetFloor=true;
							}
						}
					}
				}
			}else if(elevator_2.isNeedElevator){
				
					if(msgReceiveArr[0]==0x02){//判断是那个电梯
						
						if(msgReceiveArr[1]==0x02){
							/*
								读离散输入
							*/
							if(elevator_2.currentQueryAddress==man::DIN_REG_OPEN){
								if(msgReceiveArr[3]&0x01){
									elevator_2.isOpenedDoor=true;
								}
							}
							else if(elevator_2.currentQueryAddress==man::DIN_REG_FLOOR_1){
								if(msgReceiveArr[3]&0x01){
									elevator_2.isReachedTargetFloor=true;
								}
							}else if(elevator_2.currentQueryAddress==man::DIN_REG_FLOOR_2){
								if(msgReceiveArr[3]&0x01){
									elevator_2.isReachedTargetFloor=true;
								}
							}else if(elevator_2.currentQueryAddress==man::DIN_REG_FLOOR_3){
								if(msgReceiveArr[3]&0x01){
									elevator_2.isReachedTargetFloor=true;
								}
							}
						}
					}
					
				}

			}
			HAL_UARTEx_ReceiveToIdle_DMA(&huart3,msgReceiveArr,sizeof(msgReceiveArr));//开启DMA中断 因为DMA不是循环模式
		}
}
	
